#include "graph_optimizer/edge/edge_og.hpp"

namespace g2o
{
    EdgeOg::EdgeOg() : BaseMultiEdge<9, std::shared_ptr<graph_optimizer::PreIntegration>>()
    {
        resize(6);
        // for (int i = 0; i < 6; i++)
        // {
        //     _jacobianOplus[i].resize(9,3);
        // }
        information().setIdentity();
        // setMeasurement(nullptr);
    }
    void EdgeOg::computeError()
    {
        g2o::VertexP *Pi = (g2o::VertexP *)(_vertices[0]);
        g2o::VertexQ *Qi = (g2o::VertexQ *)(_vertices[1]);
        g2o::VertexBg *Bgi = (g2o::VertexBg *)(_vertices[2]);
        g2o::VertexP *Pj = (g2o::VertexP *)(_vertices[3]);
        g2o::VertexQ *Qj = (g2o::VertexQ *)(_vertices[4]);
        g2o::VertexBg *Bgj = (g2o::VertexBg *)(_vertices[5]);
        // if ((Bgi->estimate() - _measurement->linearized_bg_).norm() > 0.01)
        // {
        //     _measurement->repropagate(Bgi->estimate());
        //     _information = _measurement->covariance_.inverse();
        // }
        _error = _measurement->evaluate(Pi->estimate(), Qi->estimate(), Bgi->estimate(), Pj->estimate(), Qj->estimate(), Bgj->estimate());
    }
    void EdgeOg::linearizeOplus()
    {
        g2o::VertexP *Pi = (g2o::VertexP *)(_vertices[0]);
        g2o::VertexQ *Qi = (g2o::VertexQ *)(_vertices[1]);
        g2o::VertexBg *Bgi = (g2o::VertexBg *)(_vertices[2]);
        g2o::VertexP *Pj = (g2o::VertexP *)(_vertices[3]);
        g2o::VertexQ *Qj = (g2o::VertexQ *)(_vertices[4]);
        g2o::VertexBg *Bgj = (g2o::VertexBg *)(_vertices[5]);
        // if ((Bgi->estimate() - _measurement->linearized_bg_).norm() > 0.01)
        // {
        //     _measurement->repropagate(Bgi->estimate());
        //     _information = _measurement->covariance_.inverse();
        // }
        Eigen::Matrix<double, 9, 18> jacs = _measurement->jacobian(Pi->estimate(), Qi->estimate(), Bgi->estimate(), Pj->estimate(), Qj->estimate(), Bgj->estimate());
        for (unsigned int i = 0; i < _vertices.size(); i++)
        {
            _jacobianOplus[i] = jacs.block<9, 3>(0, 3 * i);
        }
    }
} // namespace g2o